#pragma once
#include "opencv2/opencv.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "slStruct.h"

#if USECUDA
#include "cuda_runtime.h"
#include "decode.cuh"
#endif

#ifndef slLog
#define slLog Log::getInstance()
#endif

#ifndef PI
#define PI 3.14159265359f
#endif

using namespace cv;

namespace SL
{
    namespace decode
    {

        int getPhase(double sinPhase, double cosPhase)
        {
            // if (sinPhase < 1e-20 && cosPhase > 0)
            //     return 0;
            // if (sinPhase > 0 && cosPhase < 1e-20)
            //     return 1;
            // if (sinPhase < 1e-20 && cosPhase < 0)
            //     return 2;
            // if (sinPhase < 0 && cosPhase < 1e-20)
            //     return 3;
            if (sinPhase > 0 && cosPhase > 0)
                return 0;
            if (sinPhase > 0 && cosPhase < 0)
                return 1;
            if (sinPhase < 0 && cosPhase < 0)
                return 2;
            if (sinPhase < 0 && cosPhase > 0)
                return 3;

            return 0;
        }

        // unsigned grayToBinary(unsigned num, unsigned numBits)
        // {
        //     for (unsigned shift = 1; shift < numBits; shift <<= 1)
        //     {
        //         num ^= num >> shift;
        //     }
        //     return num;
        // }
        int grayToBinary(int gray)
        {
            int binary = 0;
            for (; gray; gray = gray >> 1)
            {
                binary ^= gray; // binary=binary^gray;
            }
            return binary;
        }

        void gaussianFilter(Mat &image, Size kernel, float sigma)
        {
            GaussianBlur(image, image, kernel, sigma, sigma);
        }

        Mat decodePhaseShift(Images &images, Size size, int n, float phaseShiftCutoff = 15)
        {
            Mat sinSum = Mat::zeros(size.height, size.width, CV_64FC1);
            Mat cosSum = Mat::zeros(size.height, size.width, CV_64FC1);
            for (int i = 0; i < n; i++)
            {
                Mat &image = images[i];
                // gaussianFilter(image, Size(51, 51), 10.0f);
                // gaussianFilter(image, Size(101, 101), 10.0f);
                // gaussianFilter(image, Size(5, 5), 1.0f);
                for (int h = 0; h < size.height; h++)
                {
                    for (int w = 0; w < size.width; w++)
                    {
                        double x = 2 * PI * double(i) / n;
                        sinSum.at<double>(h, w) += image.at<uint8_t>(h, w) * -sin(x);
                        cosSum.at<double>(h, w) += image.at<uint8_t>(h, w) * cos(x);
                    }
                }
            }
            Mat decodeImage = Mat::zeros(size.height, size.width, CV_32FC2);
            for (int h = 0; h < size.height; h++)
            {
                for (int w = 0; w < size.width; w++)
                {

                    double sinPhase = sinSum.at<double>(h, w);
                    double cosPhase = cosSum.at<double>(h, w);
                    double tempPhase = (double)(atan(sinPhase / cosPhase));
                    int phase = getPhase(sinPhase, cosPhase);
                    double add;
                    if (phase == 0)
                        add = 0;
                    if (phase == 1)
                        add = PI;
                    if (phase == 2)
                        add = PI;
                    if (phase == 3)
                        add = 2 * PI;
                    decodeImage.at<Vec2f>(h, w)[0] = (float)(add + tempPhase);
                    float b = (float)(sqrt(sinPhase * sinPhase + cosPhase * cosPhase));
                    decodeImage.at<Vec2f>(h, w)[1] = b < phaseShiftCutoff ? 1.0f : 0;
                }
            }
            return decodeImage;
        }

        Mat decodeGrayCode(Images &images, Size size, int n, float cutoffPercentage)
        {
            Images decodeGaryImages;
            Mat decodeGrayImage = Mat::zeros(size, CV_16UC2);
            Mat minMax = Mat(size, CV_8UC2, Scalar(255, 0));
            for (int i = 0; i < n + 2; i++)
            {
                for (int h = 0; h < size.height; h++)
                {
                    for (int w = 0; w < size.width; w++)
                    {
                        if (images[i].at<uint8_t>(h, w) < minMax.at<Vec2b>(h, w)[0])
                        {
                            minMax.at<Vec2b>(h, w)[0] = images[i].at<uint8_t>(h, w);
                        }
                        if (images[i].at<uint8_t>(h, w) > minMax.at<Vec2b>(h, w)[1])
                        {
                            minMax.at<Vec2b>(h, w)[1] = images[i].at<uint8_t>(h, w);
                        }
                    }
                }
            }
            for (int i = 2; i < n + 2; i++)
            {
                Mat &image = images[i];
                Mat decodeImage = Mat::zeros(size.height, size.width, CV_8UC1);
                for (int h = 0; h < size.height; h++)
                {
                    for (int w = 0; w < size.width; w++)
                    {
                        uint8_t min = minMax.at<Vec2b>(h, w)[0];
                        uint8_t max = minMax.at<Vec2b>(h, w)[1];
                        if (max - min < 10) // 70,10
                        {
                            decodeGrayImage.at<Vec2w>(h, w)[1] = 1;
                            continue;
                        }
                        if (images[i].at<uint8_t>(h, w) > cutoffPercentage * (max - min) + min) // 0.7
                        {
                            decodeImage.at<uint8_t>(h, w) = 1;
                        }
                    }
                }
                decodeGaryImages.push_back(decodeImage);
            }
            for (int h = 0; h < size.height; h++)
            {
                for (int w = 0; w < size.width; w++)
                {
                    for (int i = 0; i < n; i++)
                    {
                        decodeGrayImage.at<Vec2w>(h, w)[0] += (bool)decodeGaryImages[i].at<uint8_t>(h, w) << (n - i - 1);
                    }
                }
            }
            return decodeGrayImage;
        }

        Mat decodeGrayCodeWithPhaseShift(Images grayImages, Images phaseShiftImages, ComplementaryGrayWithPhaseShiftInputParam param)
        {
            Size projectorSize = param.projectorSize;
            Size imageSize = param.imageSize;
            int grayCodeN = param.grayCodeN;
            int phaseShiftN = param.phaseShiftN;
            float cutoffPercentage = param.cutoffPercentage;
            float phaseShiftCutoff = param.phaseShiftCutoff;
            Mat grayDecodeImage = decodeGrayCode(grayImages, imageSize, grayCodeN, cutoffPercentage);
            Mat unwrapGrayDecodeImage = decodeGrayCode(grayImages, imageSize, grayCodeN + 1, cutoffPercentage);

            // Mat grayDecodeImage = decodeGrayCodeCuda(grayImages, imageSize, grayCodeN, cutoffPercentage);
            // Mat unwrapGrayDecodeImage = decodeGrayCodeCuda(grayImages, imageSize, grayCodeN + 1, cutoffPercentage);

            Mat phaseShiftDecodeImage = decodePhaseShift(phaseShiftImages, imageSize, phaseShiftN, phaseShiftCutoff);
            // Mat phaseShiftDecodeImage = decodePhaseShiftCuda(phaseShiftImages, imageSize, phaseShiftN);
            // Mat decodeImage = decodeGrayCodeWithPhaseShiftCuda(grayDecodeImage, unwrapGrayDecodeImage, phaseShiftDecodeImage, imageSize, projectorSize, grayCodeN);
            // return decodeImage;

            Mat decodeImage = Mat::zeros(imageSize, CV_32FC1);
            pattern::BitCodeList bitCodeList = pattern::generateGrayCode(grayCodeN);
            std::map<int, int> bitCodeMap;
            for (int i = 0; i < (1 << grayCodeN); i++)
            {
                int x = 0;
                for (int n = 0; n < grayCodeN; n++)
                {
                    x += bitCodeList[i][n] != 0 ? (1 << (grayCodeN - n - 1)) : 0;
                }
                bitCodeMap.insert(std::pair<int, int>(x, i));
            }
            pattern::BitCodeList unWrapBitCodeList = pattern::generateGrayCode(grayCodeN + 1);
            std::map<int, int> unwrapBitCodeMap;
            for (int i = 0; i < (1 << (grayCodeN + 1)); i++)
            {
                int x = 0;
                for (int n = 0; n < grayCodeN + 1; n++)
                {
                    x += unWrapBitCodeList[i][n] != 0 ? (1 << (grayCodeN - n)) : 0;
                }
                unwrapBitCodeMap.insert(std::pair<int, int>(x, i));
            }

            int stripSize = projectorSize.width / (1 << grayCodeN);
            for (int h = 0; h < imageSize.height; h++)
            {
                for (int w = 0; w < imageSize.width; w++)
                {
                    int bitcode = grayDecodeImage.at<Vec2w>(h, w)[0];
                    float uncertain = grayDecodeImage.at<Vec2w>(h, w)[1] + phaseShiftDecodeImage.at<Vec2f>(h, w)[1];
                    int unwrapBitCode = unwrapGrayDecodeImage.at<Vec2w>(h, w)[0];
                    auto iter = bitCodeMap.find(bitcode);
                    auto unwrapIter = unwrapBitCodeMap.find(unwrapBitCode);
                    if (uncertain == 0 && iter != bitCodeMap.end() && unwrapIter != unwrapBitCodeMap.end())
                    {
                        float phase = phaseShiftDecodeImage.at<Vec2f>(h, w)[0];
                        float finalPos = phase / (2 * PI) * stripSize;
                        if (true)
                        {
                            if (phase < 0.5 * PI)
                            {
                                float newPhase = phase + PI;
                                int grayId = unwrapIter->second;
                                int startStripId = grayId % 2 == 0 ? grayId - 1 : grayId;
                                decodeImage.at<float>(h, w) = newPhase / (2 * PI) * stripSize + (startStripId)*stripSize / 2.0f;
                            }
                            else if (phase > 1.5 * PI)
                            {
                                float newPhase = phase - PI;
                                int grayId = unwrapIter->second;
                                int startStripId = grayId % 2 == 0 ? grayId - 1 : grayId;
                                decodeImage.at<float>(h, w) = newPhase / (2 * PI) * stripSize + startStripId * stripSize / 2.0f;
                            }
                            else
                            {
                                decodeImage.at<float>(h, w) = finalPos + (iter->second) * stripSize;
                            }
                        }
                    }
                    else
                    {
                        decodeImage.at<float>(h, w) = NAN;
                    }
                }
            }
            return decodeImage;
        }

#if USECUDA
        Mat decodeGrayCodeWithPhaseShiftCuda(Images grayImages, Images phaseShiftImages, ComplementaryGrayWithPhaseShiftInputParam param)
        {
            Size projectorSize = param.projectorSize;
            Size imageSize = param.imageSize;
            int grayCodeN = param.grayCodeN;
            int phaseShiftN = param.phaseShiftN;
            float cutoffPercentage = param.cutoffPercentage;
            float phaseShiftCutoff = param.phaseShiftCutoff;
            Mat grayDecodeImage = SLCuda::decodeGrayCodeCuda(grayImages, imageSize, grayCodeN, cutoffPercentage);
            Mat unwrapGrayDecodeImage = SLCuda::decodeGrayCodeCuda(grayImages, imageSize, grayCodeN + 1, cutoffPercentage);
            Mat phaseShiftDecodeImage = SLCuda::decodePhaseShiftCuda(phaseShiftImages, imageSize, phaseShiftN, phaseShiftCutoff);
            Mat decodeImage = SLCuda::decodeGrayCodeWithPhaseShiftCuda(grayDecodeImage, unwrapGrayDecodeImage, phaseShiftDecodeImage, imageSize, projectorSize, grayCodeN);
            return decodeImage;
        }
#endif

        void estimateDirectLight(std::vector<Mat> images, Size imageSize, float b = 0.2)
        {
            int imageNo = (int)images.size();
            int patternNo = (imageNo - 2) / 2;
            std::vector<Mat> imagesForEstimateDirectLight;
            for (int i = 0; i < 4; i++)
            {
                Mat grayImage;
                cvtColor(images[2 + patternNo - 6 + i], grayImage, CV_BGR2GRAY);
                imagesForEstimateDirectLight.emplace_back(grayImage);
            }
            for (int i = 0; i < 4; i++)
            {
                Mat grayImage;
                cvtColor(images[2 + patternNo * 2 - 6 + i], grayImage, CV_BGR2GRAY);
                imagesForEstimateDirectLight.emplace_back(grayImage);
            }
            cv::Mat directLight(imageSize, CV_8UC2);

            float b1 = 1.0f / (1.0f - b);
            float b2 = 2.0f / (1.0f - b * 1.0f * b);

            for (int h = 0; h < imageSize.height; h++)
            {
                for (int w = 0; w < imageSize.width; w++)
                {
                    uint8_t min = 255;
                    uint8_t max = 0;
                    for (int n = 0; n < imagesForEstimateDirectLight.size(); n++)
                    {
                        uint8_t *p = &(imagesForEstimateDirectLight[n]).at<uint8_t>(h, w);
                        if (*p < min)
                            min = *p;
                        if (*p > max)
                            max = *p;
                    }
                    uint8_t ld = static_cast<uint8_t>(b1 * (max - min) + 0.5);
                    uint8_t lg = static_cast<uint8_t>(b2 * (min - b * max) + 0.5);
                    if (lg < 0)
                    {
                        ld = max;
                        lg = 0;
                    }
                    directLight.at<Vec2b>(h, w) = {ld, lg};
                }
            }
        }

        Mat decodeGrayCodeBrownWoOffset(Images &images, Size size, Size projectorSize, int n = 8, int threshold = 10, bool firstWhite = true)
        {
            std::cout << "start decode" << std::endl;
            Images &pairedImages = images;

            Mat minMax = Mat(size, CV_8UC2, Scalar(255, 0));
            Mat decodeImage = Mat::zeros(size, CV_32FC2);

            int pairNo = ((int)pairedImages.size() - 2) / 2;
            int singleDirectionNo = pairNo / 2;
            // #pragma omp parallel for
            for (int i = 0; i < pairNo; i++)
            {
                Mat &image1 = pairedImages[i * 2 + 2];
                Mat &image2 = pairedImages[i * 2 + 3];
                int channel = i < singleDirectionNo ? 0 : 1;
                int bit = i < singleDirectionNo ? i : i - singleDirectionNo;
                bit = singleDirectionNo - bit - 1;
                for (int h = 0; h < size.height; h++)
                {
                    uint8_t *image1Row = image1.ptr<uint8_t>(h);
                    uint8_t *image2Row = image2.ptr<uint8_t>(h);
                    Vec2f *decodeImageRow = decodeImage.ptr<Vec2f>(h);
                    Vec2b *minMaxRow = minMax.ptr<Vec2b>(h);
                    for (int w = 0; w < size.width; w++)
                    {
                        auto &p1 = image1Row[w];
                        auto &p2 = image2Row[w];
                        if ((firstWhite && p1 > p2) || (!firstWhite && p1 < p2))
                        {
                            decodeImageRow[w][channel] += (1 << bit);
                        }
                        uint8_t min = p1 < p2 ? p1 : p2;
                        uint8_t max = p1 > p2 ? p1 : p2;
                        if (min < minMaxRow[w][0])
                        {
                            minMaxRow[w][0] = min;
                        }
                        if (max > minMaxRow[w][1])
                        {
                            minMaxRow[w][1] = max;
                        }
                    }
                }
            }
            for (int h = 0; h < size.height; h++)
            {
                Vec2b *image1Row = minMax.ptr<Vec2b>(h);
                Vec2f *decodeImageRow = decodeImage.ptr<Vec2f>(h);
                for (int w = 0; w < size.width; w++)
                {
                    uint8_t &min = image1Row[w][0];
                    uint8_t &max = image1Row[w][1];
                    uint8_t diff = max - min;
                    if (diff < 2 * threshold)
                    {
                        decodeImageRow[w][0] = NAN;
                        decodeImageRow[w][1] = NAN;
                    }
                    else
                    {
                        int hsize = (projectorSize.width - projectorSize.width % (1 << n)) / (1 << n);
                        int vsize = (projectorSize.height - projectorSize.height % (1 << n)) / (1 << n);
                        decodeImageRow[w][0] = (float)(grayToBinary((int)decodeImage.at<Vec2f>(h, w)[0]) * hsize + hsize / 2);
                        decodeImageRow[w][1] = (float)(grayToBinary((int)decodeImage.at<Vec2f>(h, w)[1]) * vsize + vsize / 2);
                    }
                }
            }
            return decodeImage;
        }

        Mat decodeGrayCodeBrownWOffset(Images &images, Size size, Size projectorSize, int grayCodeColN = 8, int grayCodeRowN = 8, int threshold = 10, bool firstWhite = true)
        {
            std::cout << "start decode" << std::endl;
            Images &pairedImages = images;

            Mat minMax = Mat(size, CV_8UC2, Scalar(255, 0));
            Mat decodeImage = Mat::zeros(size, CV_32FC2);

            int pairNo = ((int)pairedImages.size() - 2) / 2;
            int singleDirectionNo = pairNo / 2;

            int width = projectorSize.width;
            int height = projectorSize.height;
            int hsize = width % (1 << grayCodeColN) == 0 ? (width - width % (1 << grayCodeColN)) / (1 << grayCodeColN) : (width - width % (1 << grayCodeColN)) / (1 << grayCodeColN) + 1;
            int hoffset = (hsize * (1 << grayCodeColN) - width) / 2;

            int vsize = height % (1 << grayCodeRowN) == 0 ? (height - height % (1 << grayCodeRowN)) / (1 << grayCodeRowN) : (height - height % (1 << grayCodeRowN)) / (1 << grayCodeRowN) + 1;
            int voffset = (vsize * (1 << grayCodeRowN) - height) / 2;
            // #pragma omp parallel for
            for (int i = 0; i < pairNo; i++)
            {
                Mat &image1 = pairedImages[i * 2 + 2];
                Mat &image2 = pairedImages[i * 2 + 3];
                int channel = i < singleDirectionNo ? 0 : 1;
                int bit = i < singleDirectionNo ? i : i - singleDirectionNo;
                bit = singleDirectionNo - bit - 1;
                for (int h = 0; h < size.height; h++)
                {
                    uint8_t *image1Row = image1.ptr<uint8_t>(h);
                    uint8_t *image2Row = image2.ptr<uint8_t>(h);
                    Vec2f *decodeImageRow = decodeImage.ptr<Vec2f>(h);
                    Vec2b *minMaxRow = minMax.ptr<Vec2b>(h);
                    for (int w = 0; w < size.width; w++)
                    {
                        auto &p1 = image1Row[w];
                        auto &p2 = image2Row[w];
                        if ((firstWhite && p1 > p2) || (!firstWhite && p1 < p2))
                        {
                            decodeImageRow[w][channel] += (1 << bit);
                        }
                        uint8_t min = p1 < p2 ? p1 : p2;
                        uint8_t max = p1 > p2 ? p1 : p2;
                        if (min < minMaxRow[w][0])
                        {
                            minMaxRow[w][0] = min;
                        }
                        if (max > minMaxRow[w][1])
                        {
                            minMaxRow[w][1] = max;
                        }
                    }
                }
            }
            for (int h = 0; h < size.height; h++)
            {
                Vec2b *image1Row = minMax.ptr<Vec2b>(h);
                Vec2f *decodeImageRow = decodeImage.ptr<Vec2f>(h);
                for (int w = 0; w < size.width; w++)
                {
                    uint8_t &min = image1Row[w][0];
                    uint8_t &max = image1Row[w][1];
                    uint8_t diff = max - min;
                    if (diff < 2 * threshold)
                    {
                        decodeImageRow[w][0] = NAN;
                        decodeImageRow[w][1] = NAN;
                    }
                    else
                    {
                        // std::cout << grayToBinary((int)decodeImage.at<Vec2f>(h, w)[0]) << "," << (float)(grayToBinary((int)decodeImage.at<Vec2f>(h, w)[0]) * hsize - hoffset + hsize / 2) << std::endl;
                        // std::cout << grayToBinary((int)decodeImage.at<Vec2f>(h, w)[1]) << "," << (float)(grayToBinary((int)decodeImage.at<Vec2f>(h, w)[1]) * vsize - voffset + vsize / 2) << std::endl;

                        decodeImageRow[w][0] = (float)(grayToBinary((int)decodeImage.at<Vec2f>(h, w)[0]) * hsize - hoffset + (float)hsize / 2);
                        decodeImageRow[w][1] = (float)(grayToBinary((int)decodeImage.at<Vec2f>(h, w)[1]) * vsize - voffset + (float)vsize / 2);
                        // std::cout << h << "," << w << "," << decodeImageRow[w][0] << "," << decodeImageRow[w][1] << std::endl;
                    }
                }
            }
            return decodeImage;
        }
    }
}